#!/usr/bin/env python3

import rospy
from DrRobotController_six_axes_can.msg import motor_control_set_torque
# import DrRobotController_six_axes as dr

def callback(data):
    import DrRobotController_six_axes as dr
    dr.motor_control_set_torque(id_num=data.id_num, joint_num=data.joint_num, torque=data.torque, param=data.param, mode=data.mode)
    rospy.loginfo("motor_control_set_torque_node")


def listener():
    rospy.init_node("motor_control_set_torque", anonymous=True)
    rospy.Subscriber("motor_control_set_torque", motor_control_set_torque, callback)
    rospy.spin()


if __name__ == '__main__':
    listener()

